/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/agvNavigate/offline_scurve_plan.cpp
 * @Description  : 离线S曲线规划实现，主要用于AGV的导航控制。
 *                 实现了S曲线运动规划的基本功能，包括初始化、参数设置、运动规划、关键节点计算、速度和位置获取等。
 *                 通过设置系统运动参数，可以进行离线规划，计算出从起始位置到目标位置的运动轨迹。
 *                 代码中包含了多种函数用于计算加速、减速、匀速阶段的时间和位置、速度等信息。
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 18:09:12
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/ 
#include <iostream> 
#include <vector>
#include <algorithm>
#include <iomanip>
#include <fstream>
#include <cmath>
#include "offline_scurve_plan.h"
#include "tools.h"
constexpr int MAX_ITER_FIND_V = 2000; // 最大遍历查找当前速度(根据当前反馈位置)
/**
 * @brief 构造函数，用于初始化离线S曲线计划对象
 *
 * 在构造函数中，首先调用InitSystemPara()函数初始化系统参数，
 * 然后调用Reset()函数重置对象状态。
 */
OfflineScurvePlan::OfflineScurvePlan()
{
    InitSystemPara();
    Reset();
}

/**
 * @brief 初始化系统参数
 *
 * 初始化系统参数，包括最小速度、最大速度、最小加速度、最大加速度、最小加加速度、最大加加速度、加加速度限制和周期。
 */
void OfflineScurvePlan::InitSystemPara()
{
    vmin_ = -2;
    vmax_ = 2;
    amin_ = -1;
    amax_ = 1;
    jmin_ = -1;
    jmax_ = 1;
    jlim_ = 50;
    cycle_ = 0.05f;
}

/**
 * @brief 初始化步骤标志
 *
 * 初始化当前曲线所处的阶段标志（加速、匀速、减速），并将步骤设为第一步，计划状态设为未完成。
 */
void OfflineScurvePlan::InitStepFlag()
{
    // 当前所处的曲线的阶段，区分加速、匀速、减速三段
    acc_ = uniform_ = dec_ = false;
    step_ = PlanStep::Step1;
    plan_ok_ = false;
}

/**
 * @brief 初始化关键节点
 *
 * 该函数用于初始化离线S曲线计划中的关键节点。
 *
 * 初始化包括：
 * - 8个关键的速度点（vp0_到vp7_）
 * - 8个关键的位移点（qp0_到qp7_）
 * - 加速、匀速、减速的长度（qa_、qv_、qd_）
 * - 8个关键的加速度点（ap0_到ap7_）
 *
 * 初始化后，所有值均被设置为0。
 */
void OfflineScurvePlan::InitKeyNode()
{
    // 8个关键的速度点
    vp0_ = vp1_ = vp2_ = vp3_ = vp4_ = vp5_ = vp6_ = vp7_ = 0;
    // 8个关键的位移点
    qp0_ = qp1_ = qp2_ = qp3_ = qp4_ = qp5_ = qp6_ = qp7_ = 0;
    // 加速、匀速、减速的长度
    qa_ = qv_ = qd_ = 0;
    // 8个关键的加速度点
    ap0_ = ap1_ = ap2_ = ap3_ = ap4_ = ap5_ = ap6_ = ap7_ = 0;
}

/**
 * @brief 初始化用户参数
 *
 * 该函数用于初始化用户参数，将q0_, q1_, v0_, v1_均设置为0。
 */
void OfflineScurvePlan::InitUserPara()
{
    // 用户参数
    q0_ = q1_ = 0;
    v0_ = v1_ = 0;
}

/**
 * @brief 初始化限制参数
 *
 * 根据用户参数调整的系统参数，将所有相关限制参数初始化为0。
 */
void OfflineScurvePlan::InitLimitPara()
{
    // 根据用户参数调整的系统参数
    alima_ = alimd_ = 0;
    vlima_ = vlimd_ = 0;
    vlim_ = 0; // real_vlim
    tcomp_ = 0;
    jcomp_ = 0;
}

/**
 * @brief 初始化T参数
 *
 * 初始化OfflineScurvePlan类的T参数，包括时间、速度、加速度、总时间、符号、步数、错误状态、上一步时间、索引和覆盖量等。
 */
void OfflineScurvePlan::InitTPara()
{
    tj1_ = tj2_ = 0;
    tv_ = 0;
    ta_ = td_ = 0;
    total_time_ = 0;
    sign_ = 1;
    num_steps_ = 0;
    is_error_ = 0;
    previous_t_ = 0;
    index_ = 0;
    cover_q_ = 0.0;
}

/**
 * @brief 重置离线S曲线规划
 *
 * 此函数用于重置离线S曲线规划的状态，包括步骤标志、关键节点、用户参数和T参数。
 */
void OfflineScurvePlan::Reset()
{
    InitStepFlag();
    InitKeyNode();
    InitUserPara();
    InitTPara();
}

/**
 * @brief 设置系统运动参数
 *
 * 设置离线S曲线规划的系统运动参数，包括最小速度、最大速度、最小加速度、最大加速度、最小加加速度和最大加加速度。
 *
 * @param vmin 最小速度
 * @param vmax 最大速度
 * @param amin 最小加速度
 * @param amax 最大加速度
 * @param jmin 最小加加速度
 * @param jmax 最大加加速度
 */
////////////////////////////////////////////////////////////////
// 每次重新规划需要设置必要的系统参数+Plan的位移和速度值
void OfflineScurvePlan::SetSysMotionPara(double vmin, double vmax, double amin, double amax, double jmin, double jmax)
{
    vmin_ = vmin;
    vmax_ = vmax;
    amin_ = amin;
    amax_ = amax;
    jmin_ = jmin;
    jmax_ = jmax;
}

/**
 * @brief 设置系统运动参数
 *
 * 设置离线S曲线规划的系统运动参数，包括最大速度、最大加速度和最大加加速度。
 *
 * @param vmax 最大速度，单位：米/秒
 * @param amax 最大加速度，单位：米/秒²
 * @param jmax 最大加加速度，单位：米/秒³
 */
void OfflineScurvePlan::SetSysMotionPara(double vmax, double amax, double jmax)
{
    vmin_ = -vmax;
    vmax_ = vmax;
    amin_ = -amax;
    amax_ = amax;
    jmin_ = -jmax;
    jmax_ = jmax;
}

// bool OfflineScurvePlan::Plan(double q0, double q1, double v0, double v1, int &N)
// {
//     if (fabs(q1 - q0) <= 0.01)
//     {
//         m_planOK = false;
//         DEBUG_ERROR_OUT("#2 error: q0q1 is too small;plan failed; q0=" << q0 << ",q1=" << q1 << "v0=" << v0 << "v1=" << v1);
//         return false;
//     }
//     else
//     {
//         m_planOK = true;
//         DEBUG_OUT("#2 set plan ok; q0=" << q0 << ",q1=" << q1 << "v0=" << v0 << "v1=" << v1);
//     }

//     m_index = 0;
//     m_coverQ = 0.0;
//     this->m_q0 = q0;
//     this->m_q1 = q1;
//     this->m_v0 = v0;
//     this->m_v1 = v1;
//     this->m_Tcomp = 0.0;
//     this->m_jcomp = 0.0;

//     // 考虑位移减少情况，参数转换
//     // 目前只考虑正向距离一种情况
//     Convert2OppositeCase();

//     // 当前参数满足最小位移要求
//     if (ParaMinDisRequirement())
//     {
//         // 假设Tv段存在
//         bool TvExist = TvExistTimeParaCal();
//         if (TvExist)
//         {
//             CalRealMotionPara();
//             N = m_N;
//             return true;
//         }
//         // Tv段不存在
//         double k = 1;
//         double gama = 1;
//         while (gama > 0)
//         {
//             if (TvNotExistTimeParaCal(gama))
//             {
//                 CalRealMotionPara();
//                 N = m_N;
//                 return true;
//             }
//             else
//             {
//                 // DEBUG_OUT( << "Reducing Acc" );
//                 gama *= (1 - 0.000001 * k);
//             }
//         }
//     }
//     else
//     {
//         ReCalPara();
//         return true;
//     }

//     // return false;
//     return true;
// }

/**
 * @brief 设置系统运动参数
 *
 * 设置离线S曲线规划的系统运动参数，包括最大速度、最大加速度和最大加加速度。
 *
 * @param vmax 最大速度，单位：米/秒
 * @param amax 最大加速度，单位：米/秒²
 * @param jmax 最大加加速度，单位：米/秒³
 */
bool OfflineScurvePlan::Plan(double q0, double q1, double v0, double v1)
{
    if (vmax_ < 0.0001 || amax_ < 0.0001 || jmax_ < 0.0001 || cycle_ < 0.00001) {
        plan_ok_ = false;
        DEBUG_ERROR_OUT("#0 error: vmax, amax, jmax, or cycle is too small; plan failed; "
                        << ", vmax=" << vmax_ << ", amax=" << amax_
                        << ", jmax=" << jmax_ << ", cycle=" << cycle_);
        return false;
    }
    if (fabs(q1 - q0) <= 0.01 || q0 < 0 || q1 < 0 || v0 < 0 || v1 < 0) {
        plan_ok_ = false;
        DEBUG_ERROR_OUT("#1 error: q0 or q1 is too small or q0, q1, v0, v1 have <0 value; plan failed; q0=" << q0 << ", q1=" << q1 << ", v0=" << v0 << ", v1=" << v1);
        return false;
    } else {
        plan_ok_ = true;
        DEBUG_OUT("#1 set plan ok; q0=" << q0 << ", q1=" << q1 << ", v0=" << v0 << ", v1=" << v1);
    }
    index_ = 0;
    cover_q_ = 0.0;
    q0_ = q0;
    q1_ = q1;
    v0_ = v0;
    v1_ = v1;
    tcomp_ = 0.0;
    jcomp_ = 0.0;
    // 考虑位移减少情况，参数转换
    Convert2OppositeCase();
    // 当前参数满足最小位移要求
    if (ParaMinDisRequirement()) {
        // 假设Tv段存在
        bool tv_exist = TvExistTimeParaCal();
        if (tv_exist) {
            DEBUG_OUT("Tv Exist");
            CalRealMotionPara();
            GetKeyNode();
            return true;
        }
        double k = 1;
        double gamma = 1;
        while (gamma > 0) {
            if (TvNotExistTimeParaCal(gamma)) {
                DEBUG_OUT("Tv Not Exist");
                CalRealMotionPara();
                GetKeyNode();
                return true;
            } else {
                gamma *= (1 - 0.001f * k);
                DEBUG_OUT("Reducing Acc" << ", gamma=" << gamma);
                if (gamma < 0.1) {
                    plan_ok_ = false;
                    DEBUG_ERROR_OUT("error: (while(1) crash; gamma <0.1) plan ok Reducing Acc " << ", gamma=" << gamma);
                    return false;
                }
            }
        }
    } else {
        ReCalPara();
        GetKeyNode();
        return true;
    }
    DEBUG_ERROR_OUT("ParaMinDisRequirement: plan vel is failed");
    return false;
}

/**
 * @brief 计算Tv存在的时间参数
 *
 * 该函数根据给定的初始速度v0_、最大速度vmax_、结束速度v1_、最大加速度amax_和最大减速度jmax_，
 * 计算假设Tv存在时的时间参数。如果计算出的Tv值大于等于0，则认为假设成立，返回true；
 * 否则，假设不成立，返回false。
 *
 * @return 如果计算出的Tv值大于等于0，返回true；否则返回false。
 */
bool OfflineScurvePlan::TvExistTimeParaCal()
{
    // 假设Tv存在，如果Tv不小于0，假设成立；反之，假设成功
    if ((vmax_ - v0_) * jmax_ < amax_ * amax_) {
        tj1_ = sqrt((vmax_ - v0_) / jmax_);
        ta_ = 2 * tj1_;
    } else {
        tj1_ = amax_ / jmax_;
        ta_ = tj1_ + (vmax_ - v0_) / amax_;
    }
    if ((vmax_ - v1_) * jmax_ < amax_ * amax_) {
        tj2_ = sqrt((vmax_ - v1_) / jmax_);
        td_ = 2 * tj2_;
    } else {
        tj2_ = amax_ / jmax_;
        td_ = tj2_ + (vmax_ - v1_) / amax_;
    }
    tv_ = (q1_ - q0_) / vmax_ - 0.5 * ta_ * (1 + v0_ / vmax_) - 0.5 * td_ * (1 + v1_ / vmax_);
    return tv_ >= 0;
}

/**
 * @brief 计算当Tv不存在时的时间参数
 *
 * 根据给定的加速度峰值gama，计算相关时间参数，包括加速时间ta_、减速时间td_、加速段持续时间tj1_、减速段持续时间tj2_。
 *
 * @param gama 加速度峰值
 *
 * @return 若计算成功，返回true；否则返回false
 */
bool OfflineScurvePlan::TvNotExistTimeParaCal(double gama)
{
    amax_ = gama * amax_;
    tv_ = 0;
    tj1_ = tj2_ = amax_ / jmax_;
    double delta = pow(amax_, 4) / pow(jmax_, 2) + 2 * (v0_ * v0_ + v1_ * v1_) +
                   amax_ * (4 * (q1_ - q0_) - 2 * (amax_ / jmax_) * (v0_ + v1_));
    ta_ = (amax_ * amax_ / jmax_ - 2 * v0_ + sqrt(delta)) / (2 * amax_);
    td_ = (amax_ * amax_ / jmax_ - 2 * v1_ + sqrt(delta)) / (2 * amax_);
    if (ta_ >= 2 * tj1_ && td_ >= 2 * tj2_) {
        DEBUG_OUT("TvNotExistTimeParaCal ok: Ta= " << ta_ << ", Td=" << td_ << ", Tj1=" << tj1_ << ", Tj2=" << tj2_);
        return true;
    } else {
        if (ta_ < 0) {
            ta_ = 0;
            tj1_ = 0;
            td_ = 2 * ((q1_ - q0_) / (v1_ + v0_));
            if (jmax_ * (jmax_ * pow(q1_ - q0_, 2) + pow(v1_ + v0_, 2) * (v1_ - v0_)) < 0) {
                is_error_ = -1;
                DEBUG_OUT(", TvNotExistTimeParaCal failed, error=1; Ta<0");
                return true;
            }
            tj2_ = (jmax_ * (q1_ - q0_) - sqrt(jmax_ * (jmax_ * pow(q1_ - q0_, 2) + pow(v1_ + v0_, 2) * (v1_ - v0_)))) /
                   (jmax_ * (v1_ + v0_));
            return true;
        }
        if (td_ < 0) {
            td_ = 0;
            tj2_ = 0;
            ta_ = 2 * ((q1_ - q0_) / (v1_ + v0_));
            if (jmax_ * (jmax_ * pow(q1_ - q0_, 2) + pow(v1_ + v0_, 2) * (v1_ - v0_)) < 0) {
                is_error_ = -1;
                DEBUG_OUT(", TvNotExistTimeParaCal failed, error=2; Td<0");
                return true;
            }
            tj1_ = (jmax_ * (q1_ - q0_) - sqrt(jmax_ * (jmax_ * pow(q1_ - q0_, 2) - pow(v1_ + v0_, 2) * (v1_ - v0_)))) /
                   (jmax_ * (v1_ + v0_));
            return true;
        }
        return false;
    }
}

/**
 * @brief 设置周期时间
 *
 * 设置当前离线S曲线计划对象的周期时间。
 *
 * @param cycle 周期时间（单位：秒）
 */
void OfflineScurvePlan::SetCycle(double cycle)
{
    cycle_ = cycle;
}

/**
 * @brief 判断系统参数是否满足用户设定的最小距离要求
 *
 * 该函数根据当前系统的最大加速度 jmax_ 和最大减速度 amax_，以及用户设定的初始速度 v0_、目标速度 v1_、初始位置 q0_ 和目标位置 q1_，
 * 判断系统是否能在满足加速度和减速度限制的情况下，从初始位置 q0_ 以初始速度 v0_ 移动到目标位置 q1_ 并达到目标速度 v1_。
 *
 * @return 如果满足要求，则返回 true；否则返回 false。
 */
//  ,vmax=0.4,amax=1,jmax=2,q0=2.42817,q1=3.14159,v0=1.13,v1=0,type=3,endId=247,endId=247
bool OfflineScurvePlan::ParaMinDisRequirement()
{
    // Tjstart1 尚未达到最大加速度单脉冲持续时间；Tjstar2达到最大加速度单脉冲持续时间
    double tjstar1 = sqrt(fabs(v1_ - v0_) / jmax_);
    double tjstar2 = amax_ / jmax_;
    // 选择一个最小的
    double tjstar = std::min(tjstar1, tjstar2);
    double min_q0q1;

    if (tjstar == tjstar1) {
        min_q0q1 = tjstar * (v0_ + v1_);
    } else {
        min_q0q1 = 0.5 * (v0_ + v1_) * (tjstar + fabs(v1_ - v0_) / amax_);
    }
    // 当计算减速距离时，可能出来相等的情况
    if (q1_ - q0_ >= min_q0q1) {
        return true;
    }
    DEBUG_OUT("ERROR: ParaMinDisRequirement: The current system parameters (jmax, amax) "
              << "cannot meet the requirements of user settings (q0, q1, v0, v1). "
              << "Solution: Improve system performance or modify user parameters.");
    DEBUG_OUT("ParaMinDisRequirement: failed: v1=" << v1_ << ", v0=" << v0_ << ", jmax=" << jmax_ << ", amax=" << amax_ << ", Tjstar1=" << tjstar1
                                                   << ", Tjstar2=" << tjstar2
                                                   << ", q1=" << q1_ << ", q0=" << q0_ << ", min_q0q1=" << min_q0q1);
    return false;
}

/**
 * @brief 重新计算参数
 *
 * 当无法满足条件时，分为两种情况：只加速或只减速，并且距离不足。
 *
 * 如果当前剩余距离不足以正常减速，则只存在DP1和DP3段，需要重新计算jerk值。
 * 如果当前剩余距离不足以加速到目标速度，则忽略距离限制，使用最大急动和加速度。
 */
void OfflineScurvePlan::ReCalPara() /*无法满足条件分为两种，只加速或者只减速,且距离不够*/
{
    double tj, jerk;
    /* 第1种：当前的剩余距离不足以正常减速，只存在DP1和DP3段，需要重新计算jerk  */
    if (fabs(v0_) >= fabs(v1_)) {
        tj = fabs(q1_ - q0_) / fabs(v1_ + v0_);
        DEBUG_OUT("RecalculateParameters: dp: jerk t=" << tj);
        jerk = fabs(v1_ - v0_) / pow(tj, 2);
        // 如果计算的急动超过限制，则以限制值计算
        if (jerk > jlim_) {
            jerk = jlim_;
            tj = sqrt(fabs(v1_ - v0_) / jerk);
        }
        DEBUG_OUT("RecalculateParameters: v0 > v1; Only DP1 and DP3 segments exist. Recalculate jerk=" << jerk
                                      << ", v0=" << v0_ << ", v1=" << v1_);
        jmax_ = jerk;
        jmin_ = -jerk;
        alima_ = 0;
        alimd_ = -tj * jmax_;
        tj1_ = 0;
        tj2_ = tj;
        tv_ = 0;
        ta_ = 0;
        td_ = 2 * tj;
        total_time_ = td_;
        vlim_ = v0_ + (ta_ - tj1_) * alima_;
        num_steps_ = static_cast<int>(total_time_ / cycle_ + 1);
    } else if (fabs(v0_) < fabs(v1_)) {/*第2种：当前的剩余距离不足以加速到目标速度，则忽略距离限制，使用最大急动和加速度 */
        tj = sqrt(fabs(v1_ - v0_) / jmax_);
        DEBUG_OUT("RecalculateParameters: v0 < v1; Only AP1 and AP3");
        alima_ = tj * jmax_;
        alimd_ = 0;
        tj1_ = tj;
        tj2_ = 0;
        tv_ = 0;
        ta_ = 2 * tj;
        td_ = 0;
        total_time_ = ta_;
        vlim_ = v0_ + (ta_ - tj1_) * alima_;
        num_steps_ = static_cast<int>(total_time_ / cycle_ + 1);
    }
    DEBUG_OUT("[RePlan Info]:");
    DEBUG_OUT("alima = " << alima_);
    DEBUG_OUT("alimd = " << alimd_);
    DEBUG_OUT("vlim = " << sign_ * vlim_);
    ShowKeyTime();
}

/**
 * @brief 将变量转换为相反的大小写
 *
 * 将对象的所有变量（包括位置、速度、加速度和加加速度）转换为相反的大小写。
 * 如果 q1_ 大于 q0_，则将符号设置为 1，否则设置为 -1。
 *
 */
void OfflineScurvePlan::Convert2OppositeCase()
{
    if (q1_ > q0_) {
        sign_ = 1;
    } else {
        sign_ = -1;
    }
    q0_ = sign_ * q0_;
    q1_ = sign_ * q1_;
    v0_ = sign_ * v0_;
    v1_ = sign_ * v1_;
    double k1 = (sign_ + 1) / 2;
    double k2 = (sign_ - 1) / 2;
    vmax_ = k1 * vmax_ + k2 * vmin_;
    vmin_ = k1 * vmin_ + k2 * vmax_;
    amax_ = k1 * amax_ + k2 * amin_;
    amin_ = k1 * amin_ + k2 * amax_;
    jmax_ = k1 * jmax_ + k2 * jmin_;
    jmin_ = k1 * jmin_ + k2 * jmax_;
}

/**
 * @brief 计算实际运动参数
 *
 * 该函数用于计算离线S曲线规划中的实际运动参数。
 *
 * @details
 * 该函数主要完成以下计算：
 * 1. 计算最大加速度 alima_ 和最大减速度 alimd_。
 * 2. 计算最大速度 vlim_。
 * 3. 计算总时间 total_time_。
 * 4. 计算步数 num_steps_。
 * 5. 打印出关键参数信息。
 */
void OfflineScurvePlan::CalRealMotionPara()
{
   alima_ = jmax_ * tj1_;
    alimd_ = -jmax_ * tj2_;
    vlim_ = v0_ + (ta_ - tj1_) * alima_;
    total_time_ = ta_ + td_ + tv_;
    num_steps_ = static_cast<int>(total_time_ / cycle_ + 1);
    DEBUG_OUT("[Plan Info]:");
    DEBUG_OUT("alima = " << alima_);
    DEBUG_OUT("alimd = " << alimd_);
    DEBUG_OUT("vlim = " << sign_ * vlim_);
    ShowKeyTime();
}

/**
 * @brief 显示关键时间点
 *
 * 该函数用于在调试模式下打印出关键的时间点信息，以便于调试和验证离线S曲线计划。
 *
 * @note 该函数仅在调试模式下有效。
 */
void OfflineScurvePlan::ShowKeyTime()
{
    DEBUG_OUT("ShowKeyTimes:  [" << 0 << ", Tj1=" << tj1_
                                  << ", Ta-Tj1=" << ta_ - tj1_ << ", Ta=" << ta_
                                  << ", Ta+Tv=" << ta_ + tv_ << ", T-Td+Tj2(T5)=" << total_time_ - td_ + tj2_
                                  << ", T=" << total_time_ << "]");
}

/**
 * @brief 根据时间ti计算位置qi
 *
 * 根据给定的时间ti，计算对应的位置qi。
 *
 * @param ti 给定的时间
 * @param qi 计算得到的位置
 * @return 计算成功返回true，否则返回false
 */
bool OfflineScurvePlan::GetQi(double ti, double& qi) const
{
     if (ti >= 0 && ti < tj1_) {
        qi = q0_ + v0_ * ti + jmax_ * pow(ti, 3) / 6;
    } else if (ti >= tj1_ && ti < ta_ - tj1_) {
        qi = q0_ + v0_ * ti + (alima_ / 6) * (3 * ti * ti - 3 * tj1_ * ti + tj1_ * tj1_);
    } else if (ti >= ta_ - tj1_ && ti < ta_) {
        qi = q0_ + (vlim_ + v0_) * (ta_ / 2) - vlim_ * (ta_ - ti) -
             jmin_ * pow(ta_ - ti, 3) / 6;
    } else if (ti >= ta_ && ti < ta_ + tv_) {
        qi = q0_ + (vlim_ + v0_) * (ta_ / 2) + vlim_ * (ti - ta_);
    } else if ((ti >= total_time_ - td_ || ti >= ta_ + tv_) && ti < total_time_ - td_ + tj2_) {
        qi = q1_ - (vlim_ + v1_) * (td_ / 2) + vlim_ * (ti - total_time_ + td_) - jmax_ * pow(ti - total_time_ + td_, 3) / 6;
    } else if (ti >= total_time_ - td_ + tj2_ && ti < total_time_ - tj2_) {
        qi = q1_ - (vlim_ + v1_) * (td_ / 2) + vlim_ * (ti - total_time_ + td_) +
             (alimd_ / 6) * (3 * pow(ti - total_time_ + td_, 2) - 3 * tj2_ * (ti - total_time_ + td_) + pow(tj2_, 2));
    } else if (ti >= total_time_ - tj2_ && ti <= total_time_) {
        qi = q1_ - v1_ * (total_time_ - ti) - jmax_ * pow(total_time_ - ti, 3) / 6;
    } else if (ti >= total_time_ && ti <= total_time_ + tcomp_) {
        qi = q1_;
    } else {
        DEBUG_ERROR_OUT("error: GetQi: ti=" << ti << ", total_time_=" << total_time_ << ", total_time_ + tcomp_=" << total_time_ + tcomp_);
        return false;
    }
    return true;
}

/**
 * @brief 根据给定的时间 ti 获取速度 vi
 *
 * 根据给定的时间 ti，计算并返回对应的速度 vi。
 *
 * @param ti 给定的时间
 * @param vi 返回的速度值，通过引用传递
 * @return 如果计算成功，返回 true；如果失败，返回 false
 */
bool OfflineScurvePlan::GetVi(double ti, double &vi)
{
    double delta_t;
    double t2 = (ta_ - 2 * tj1_) > 0 ? (ta_ - 2 * tj1_) : 0.0;
    double t3 = (td_ - 2 * tj2_) > 0 ? (td_ - 2 * tj2_) : 0.0;
    double vmax = v0_ + jmax_ * tj1_ * (tj1_ + t2);
    if (ti >= 0 && ti < tj1_) {
        vi = v0_ + jmax_ * pow(ti, 2) / 2;
        acc_ = true;
        uniform_ = false;
        dec_ = false;
        step_ = PlanStep::Step1;
    } else if (ti >= tj1_ && ti < ta_ - tj1_) {
        delta_t = ti - tj1_;
        vi = v0_ + 0.5 * jmax_ * tj1_ * tj1_ + jmax_ * tj1_ * delta_t;
        acc_ = true;
        uniform_ = false;
        dec_ = false;
        step_ = PlanStep::Step2;
    } else if (ti >= ta_ - tj1_ && ti < ta_) {
        delta_t = ti + tj1_ - ta_;
        vi = v0_ + jmax_ * (tj1_ * t2 + 0.5 * pow(tj1_, 2) + tj1_ * delta_t - 0.5 * pow(delta_t, 2));
        acc_ = true;
        uniform_ = false;
        dec_ = false;
        step_ = PlanStep::Step3;
    } else if (ti >= ta_ && ti < ta_ + tv_) {
        vi = vmax;
        acc_ = false;
        uniform_ = true;
        dec_ = false;
        step_ = PlanStep::Step4;
    } else if ((ti >= total_time_ - td_ || ti >= ta_ + tv_) && ti < total_time_ - td_ + tj2_) {
        delta_t = ti - ta_ - tv_;
        vi = vmax - 0.5 * jmax_ * pow(delta_t, 2);
        acc_ = false;
        uniform_ = false;
        dec_ = true;
        step_ = PlanStep::Step5;
    } else if (ti >= total_time_ - td_ + tj2_ && ti < total_time_ - tj2_) {
        delta_t = ti - ta_ - tv_ - tj2_;
        vi = vmax - 0.5 * jmax_ * pow(tj2_, 2) - tj2_ * jmax_ * delta_t;
        acc_ = false;
        uniform_ = false;
        dec_ = true;
        step_ = PlanStep::Step6;
    } else if (ti >= total_time_ - tj2_ && ti <= total_time_ + tcomp_) {
        delta_t = ti - total_time_ + tj2_;
        vi = vmax - 0.5 * jmax_ * pow(tj2_, 2) - tj2_ * jmax_ * (t3) - (tj2_ + tcomp_) * (jmax_ + jcomp_) * delta_t + 0.5 * (jmax_ + jcomp_) * pow(delta_t, 2);
        ap6_ = jmax_ * tj2_;
        acc_ = false;
        uniform_ = false;
        dec_ = true;
        step_ = PlanStep::Step7;
    } else {
        vi = 0;
        step_ = PlanStep::Step7;
        DEBUG_ERROR_OUT("error: GetVi: ti=" << ti << ", vi=" << vi);
        return false;
    }
    return true;
}

/**
 * @brief 结束补偿函数
 *
 * 根据剩余距离计算结束补偿的时间和加加速度
 *
 * @param surplus_distance 剩余距离
 */
// EndCompensation: dis=0.005,m_Tcomp=-0.0767949,m_jcomp=1.7735,T_new=0.173205,m_Tj2=0.25,jerk_new=5.7735,m_jmax=4
void OfflineScurvePlan::EndCompensation(double surplus_distance)
{
    if (surplus_distance > 0 && (jmax_ * tj2_) > 0) {
        double t_new = 3 * surplus_distance / (0.5 * jmax_ * tj2_ * tj2_);
        double jerk_new = 6 * surplus_distance / pow(t_new, 3);
        tcomp_ = t_new - tj2_;
        jcomp_ = jerk_new - jmax_;
        DEBUG_OUT("EndCompensation: dis=" << surplus_distance
                                          << ", tcomp_=" << tcomp_ << ", jcomp_=" << jcomp_
                                          << ", t_new=" << t_new << ", tj2_=" << tj2_
                                          << ", jerk_new=" << jerk_new << ", jmax_=" << jmax_);
    }
}

/**
 * @brief 获取当前步骤数
 *
 * 获取当前步骤数。
 *
 * @return 返回当前步骤数
 */
int OfflineScurvePlan::GetStep() const
{
    return step_;
}

/**
 * @brief 移动到指定位置
 *
 * 根据给定的索引 i，计算并返回在离线 S 曲线计划中的位置 qi。
 *
 * @param i 索引，表示目标位置在 S 曲线中的位置索引
 * @param qi [out] 返回计算得到的 qi 值
 *
 * @return 如果成功计算并返回 qi 值，则返回 true；否则返回 false
 *
 * @note 此函数首先计算时间 ti，然后根据时间 ti 获取对应的 qi 值。
 *       如果 ti 超过了总时间 total_time_ 加上补偿时间 tcomp_，则将 ti 设置为 total_time_ + tcomp_。
 *       然后调用 GetQi 函数计算 qi 值，并根据符号 sign_ 对 qi 进行调整。
 */
bool OfflineScurvePlan::Move(int i, double &qi)
{
    double ti = cycle_ * i;
    if (ti > total_time_ + tcomp_) {
        ti = total_time_ + tcomp_;
    }
    if (!GetQi(ti, qi)) {
        return false;
    }
    qi *= sign_;
    return true;
}
/**
 * @brief 根据给定的时间索引i，计算对应的位移qi和速度vi
 *
 * 根据时间索引i计算对应的位移qi和速度vi。
 *
 * @param i 时间索引
 * @param qi 引用参数，用于存储计算出的位移
 * @param vi 引用参数，用于存储计算出的速度
 *
 * @return 如果计算成功返回true，否则返回false
 */
bool OfflineScurvePlan::Move(int i, double &qi, double &vi)
{
    double ti = cycle_ * i;
    if (ti > total_time_ + tcomp_) {
        ti = total_time_ + tcomp_;
    }
    double previous_qi = qi;
    if (!GetQi(ti, qi)) {
        return false;
    }
    qi *= sign_;
    if (i == 0) {
        vi = v0_;
    } else {
        vi = (qi - previous_qi) / cycle_;
    }
    return true;
}

/**
 * @brief 根据时间t更新位置qi和速度vi
 *
 * 根据给定时间t，更新物体的位置qi和速度vi。
 *
 * @param t 当前时间
 * @param qi 位置引用，用于存储更新后的位置
 * @param vi 速度引用，用于存储更新后的速度
 * @return 更新成功返回true，否则返回false
 */
bool OfflineScurvePlan::Move(double t, double &qi, double &vi)
{
    if (t > total_time_ + tcomp_) {
        t = total_time_ + tcomp_;
    }
    double previous_qi = qi;
    double previous_vi = vi;
    if (!GetQi(t, qi)) {
        return false;
    }
    qi *= sign_;
    if (t == 0) {
        vi = v0_;
    } else if (fabs(t - previous_t_) < 0.0001) {
        vi = previous_vi;
    } else {
        vi = (qi - previous_qi) / (t - previous_t_);
    }
    previous_t_ = t;
    return true;
}

/**
 * @brief 根据给定时间点计算速度
 *
 * 根据给定的时间点i计算对应的速度，并返回计算是否成功。
 *
 * @param i 给定的时间点，以循环次数为单位
 * @param vi 计算出的速度值，通过引用传递
 *
 * @return 如果计算成功，返回true；否则返回false
 */
bool OfflineScurvePlan::MoveVelocity(int i, double &vi)
{
    double ti = cycle_ * i;
    if (ti > total_time_ + tcomp_) {
        ti = total_time_ + tcomp_;
    }
    if (!GetVi(ti, vi)) {
        return false;
    }
    vi *= sign_;
    return true;
}

/**
 * @brief 根据给定时间计算速度
 *
 * 根据给定的时间 t，计算在该时间点的速度，并存储在 vi 中。
 *
 * @param t 给定的时间
 * @param vi 计算出的速度值，通过引用传递
 *
 * @return 如果计算成功，返回 true；否则返回 false
 */
bool OfflineScurvePlan::MoveVelocity(double t, double &vi)
{
    if (t > total_time_ + tcomp_) {
        t = total_time_ + tcomp_;
    }
    if (!GetVi(t, vi)) {
        return false;
    }
    vi *= sign_;
    return true;
}

/**
 * @brief 获取离线S曲线计划的关键节点
 *
 * 该函数用于计算离线S曲线计划的关键节点，并通过DEBUG_OUT宏打印关键节点的速度和时间信息。
 *
 * 首先，使用MoveVelocity函数计算每个时间点的速度，并设置相应的速度值。然后，使用Move函数计算每个时间点的位置，并临时存储速度值。
 * 最后，计算关键位置向量qa_、qv_和qd_。
 *
 * @note 此函数通过DEBUG_OUT宏打印关键节点的速度和时间信息，方便调试。
 */
void OfflineScurvePlan::GetKeyNode()
{
    MoveVelocity(0.0f, vp0_);
    MoveVelocity(tj1_, vp1_);
    MoveVelocity(ta_ - tj1_, vp2_);
    MoveVelocity(ta_, vp3_);
    MoveVelocity(tv_ + ta_, vp4_);
    MoveVelocity(total_time_ - td_ + tj2_, vp5_);
    MoveVelocity(total_time_ - tj2_, vp6_);
    MoveVelocity(total_time_ + tcomp_, vp7_);
    DEBUG_OUT("GetKeyNodes: total time = " << total_time_);
    DEBUG_OUT("GetKeyNodes:  v0=" << vp0_ << ", v1=" << vp1_
                                   << ", v2=" << vp2_ << ", v3=" << vp3_
                                   << ", v4=" << vp4_ << ", v5=" << vp5_
                                   << ", v6=" << vp6_ << ", v7=" << vp7_);
    double temp_v;
    Move(0.0, qp0_, temp_v);
    Move(tj1_, qp1_, temp_v);
    Move(ta_ - tj1_, qp2_, temp_v);
    Move(ta_, qp3_, temp_v);
    Move(tv_ + ta_, qp4_, temp_v);
    Move(total_time_ - td_ + tj2_, qp5_, temp_v);
    Move(total_time_ - tj2_, qp6_, temp_v);
    Move(total_time_ + tcomp_, qp7_, temp_v);
    qa_ = qp3_ - qp0_;
    qv_ = qp4_ - qp3_;
    qd_ = qp7_ - qp4_;
    DEBUG_OUT("GetKeyNodes: q0=" << qp0_ << ", q1=" << qp1_
                                 << ", q2=" << qp2_ << ", q3=" << qp3_
                                 << ", q4=" << qp4_ << ", q5=" << qp5_
                                 << ", q6=" << qp6_ << ", q7=" << qp7_);
}

// 假设距离足够的情况,只给定初始和结束速度，计算加速和减速距离
// bool OfflineScurvePlan::Plan(double v0, double v1)
// {
//     int N = 0;
//     bool ret = Plan(0, 1000, v0, v1, N);
//     if (!ret)
//     {
//         DEBUG_OUT("error: Plan v0=" << v0 << ",v1=" << v1);
//     }
//     GetKeyNode();
//     DEBUG_OUT("v0=" << v1 << ",v1=" << v1 << "ret=" << ret);
//     return ret;
// }

/**
 * @brief 获取规划速度
 *
 * 根据当前规划的进度，返回当前的速度值。
 *
 * @return 返回当前规划的速度值，类型为 double。
 */
// 调用一次，获取一次，速度变化一次，索引递增
double OfflineScurvePlan::PlanVel()
{
    double vi = 0;
    if (!plan_ok_) {
        step_ = PlanStep::Step1;
        DEBUG_ERROR_OUT("error: current plan failed; force plan vel is 0; reset step to Step1");
        return 0;
    }
    if (index_ < 0) {
        index_ = 0;
    }
    index_++;
    MoveVelocity(index_, vi);
    DEBUG_OUT("plan vel: index=" << index_ << ", vi=" << vi << ", size=" << static_cast<int>(Size()));
    return vi;
}

/**
 * @brief 获取计划区域
 *
 * 根据给定的索引和周期计算计划区域，并输出调试信息
 *
 * @param rel_qi [out] 相对距离（输出参数）
 * @param total_qi [out] 总距离（输出参数）
 */
void OfflineScurvePlan::GetPlanArea(double& rel_qi, double& total_qi)
{
     double ti = cycle_ * index_, vi_diff = 0.0;
    Move(ti, total_qi, vi_diff);
    rel_qi = total_qi - cover_q_;
    DEBUG_OUT("plan area: index=" << index_ << ", ti=" << ti << ", vi_diff=" << vi_diff
                                  << ", rel_qi=" << rel_qi << ", total_qi=" << total_qi << ", q0=" << q0_ << ", q1=" << q1_);
}

/**
 * @brief 向离线S曲线计划中添加新的覆盖量Q
 *
 * 该函数将新的覆盖量Q添加到离线S曲线计划中的总覆盖量中，并返回更新后的总覆盖量。
 *
 * @param add_q 要添加的新覆盖量Q
 * @return 更新后的总覆盖量
 */
double OfflineScurvePlan::AddNewCoverQ(double add_q)
{
    cover_q_ += add_q;
    DEBUG_OUT("add new Q=" << add_q << ", total coverQ=" << cover_q_);
    return cover_q_;
}

/**
 * @brief 根据给定时间 t 获取位置 qi
 *
 * 根据给定的时间 t，计算出对应的位置 qi，并将结果存储在 qi 参数中。
 *
 * @param t 时间（秒）
 * @param qi 存储计算出的位置
 *
 * @return 成功时返回 true，失败时返回 false
 *
 * 注意：
 * - 如果 t 大于总时间 total_time_ 和补偿时间 tcomp_ 之和，t 将被设置为 total_time_ + tcomp_。
 * - 如果 t 小于 0，t 将被设置为 0。
 * - 如果 GetQi 函数调用失败，则函数返回 false，否则将 qi 乘以符号 sign_ 并返回 true。
 */
bool OfflineScurvePlan::GetPosition(double t, double& qi) const
{
    if (t > total_time_ + tcomp_) {
        t = total_time_ + tcomp_;
    } else if (t < 0) {
        t = 0;
    }
    if (!GetQi(t, qi)) {
        return false;
    }
    qi *= sign_;
    return true;
}

/**
 * @brief 根据目标位置查找对应的时间
 *
 * 该函数通过二分查找算法，在给定的时间范围内查找与目标位置对应的时间。
 *
 * @param target_q 目标位置
 * @return 找到的时间点
 */
double OfflineScurvePlan::FindT(double target_q ) const
{
    // 预设起点和终点时间
    double start_t = 0;
    double end_t = total_time_ + tcomp_;
    double max_t = total_time_ + tcomp_;
    double t = (start_t + end_t) / 2.0;
    double q = 0;
    bool ret = GetPosition(t, q);
    double err = fabs(target_q - q);
    std::cout << "ComputeElpTime: start" << ", first GetPosition ret=" << ret << ", t=" << t << ", q=" << q << std::endl;
    auto start_clock = std::chrono::steady_clock::now();
    int cycle_times = 0;
    while (err > 0.004) {
        cycle_times++;
        if (target_q < q) {
            end_t = t;
        } else {
            start_t = t;
        }
        t = (start_t + end_t) / 2.0f;
        ret = GetPosition(t, q);
        err = fabs(target_q - q);
        DEBUG_OUT("FindT: cycle_times=" << cycle_times
                                        << ", err=" << err
                                        << ", target_q=" << target_q
                                        << ", GetPosition ret=" << ret
                                        << ", q=" << q
                                        << ", t=" << t
                                        << ", start_t=" << start_t
                                        << ", end_t=" << end_t);
        if (std::abs(start_t) <= 0.00001 && std::abs(end_t) <= 0.00001) {
            DEBUG_ERROR_OUT("error: FindT failed; minT; set t=" << t);
            break;
        }
        if (std::abs(start_t - max_t) <= 0.00001 && std::abs(end_t - max_t) <= 0.00001) {
            DEBUG_ERROR_OUT("error: FindT failed; maxT; set t=" << t);
            break;
        }
        if (cycle_times >= MAX_ITER_FIND_V) {
            DEBUG_ERROR_OUT("error: FindT failed; set plan flag false cycle_times is too large; target_q=" << target_q);
            break;
        }
    }
    std::chrono::duration<double> diff_clock = std::chrono::steady_clock::now() - start_clock;
    DEBUG_OUT("end elapsed time: " << diff_clock.count() * 1000 << "ms");
    return t;
}

/**
 * @brief 根据当前位置找到速度
 *
 * 根据当前位置 cur_q 计算速度 v。
 *
 * @param cur_q 当前位置
 * @return 计算得到的速度值，如果无法找到速度则返回 0
 */
double OfflineScurvePlan::FindV(double cur_q)
{
    double v = 0.0;
    double prog = cover_q_ + cur_q;
    double t = FindT(prog);
    bool find = MoveVelocity(t + cycle_ * 2, v);
    DEBUG_OUT("FindV: v=" << v
                          << ", prog=" << prog
                          << ", t=" << t
                          << ", t+cycle*2=" << (t + cycle_ * 2)
                          << ", cur_q=" << cur_q
                          << ", cover_q_=" << cover_q_
                          << ", find=" << find);
    return find ? v : 0;
}

/**
 * @brief 获取停止距离
 *
 * 根据当前速度v计算停止距离。
 *
 * @param v 当前速度
 *
 * @return 停止距离
 *
 * @note 在计算停止距离之前，需要验证m_amax和m_jmax是否有效。
 * 如果不验证，则可能使用默认值进行计算。
 */
double OfflineScurvePlan::GetStopDistance(double v) const
{
    // FIXME:停止距离判断前，验证是否m_amax 和m_jmax有效，先运行在减速度距离判断，否则可能是默认值
    double v_threshold = pow(amax_, 2) / jmax_;
    double dec_distance = 0.0;
    if (v <= v_threshold) {
        double t = sqrt(v / jmax_);
        dec_distance = t * v;
        DEBUG_OUT("GetStopDistance v_threshold=" << v_threshold << ", dec_distance=" << dec_distance
                                                 << ", t=" << t << ", v=" << v);
    } else {
        double t1 = amax_ / jmax_;
        double t3 = t1;
        double t2 = (v - pow(amax_, 2) / jmax_) / amax_;
        double v1 = 0.5 * pow(amax_, 2) / jmax_;
        double v2 = v - 0.5 * pow(amax_, 2) / jmax_;
        dec_distance = 0.5 * t1 * v1 + 0.5 * (v1 + v2) * t2 + 0.5 * (v2 + v) * t3;
        DEBUG_OUT("GetStopDistance v_threshold=" << v_threshold
                                                 << ", dec_distance=" << dec_distance
                                                 << ", t1=" << t1
                                                 << ", t3=" << t3
                                                 << ", t2=" << t2
                                                 << ", v1=" << v1
                                                 << ", v2=" << v2);
    }
    return dec_distance;
}

/**
 * @brief 计算位移需求
 *
 * 根据初始速度、目标速度、最大加速度和最大加加速度计算位移需求
 *
 * @param choose_min 是否选择最小位移需求
 * @param v0 初始速度
 * @param v1 目标速度
 * @param amax 最大加速度
 * @param jmax 最大加加速度
 *
 * @return 位移需求
 */
double OfflineScurvePlan::DisRequirement(bool choose_min, double v0, double v1, double amax, double jmax)
{
    // Tjstart1 尚未达到最大加速度单脉冲持续时间；Tjstar2达到最大加速度单脉冲持续时间
    double tjstar1 = sqrt(fabs(v1 - v0) / jmax);
    double tjstar2 = amax / jmax;
    double q0q1 = 0;
    if (choose_min) {
        q0q1 = tjstar1 * (v0 + v1_);
    } else {
        q0q1 = 0.5 * (v0 + v1) * (tjstar2 + fabs(v1 - v0) / amax);
    }
    DEBUG_OUT("#1 DisRequirement" << ", v0=" << v0
                                  << ", v1=" << v1
                                  << ", jmax=" << jmax
                                  << ", amax=" << amax
                                  << ", Tjstar1=" << tjstar1
                                  << ", Tjstar2=" << tjstar2
                                  << ", q0q1=" << q0q1);
    return q0q1;
}

/**
 * @brief 计算位移
 *
 * 根据加速度最大值、加加速度最大值、初始速度和目标速度，计算位移。
 *
 * @param choose_min 是否选择最小位移。如果为true，则选择最小位移；否则选择最大位移。
 * @param v0 初始速度
 * @param v1 目标速度
 *
 * @return 返回位移值
 */
double OfflineScurvePlan::DisRequirement(bool choose_min, double v0, double v1)
{
    DEBUG_OUT("#2 DisRequirement: amax=" << amax_ << ", jmax=" << jmax_);
    double dis = DisRequirement(choose_min, v0, v1, amax_, jmax_);
    return dis;
}
